﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

namespace Pacman.Simulator
{
    class aStar
    {
        List<aNode> openList; // Frontier nodes
        List<aNode> closedList; // Already evaluated
        List<aNode> path;
        Boolean goalFound = false;
        aNode start;
        aNode goal;
        private const int width = 28;
        private const int height = 31;
        int[,] influenceMap;

        public aStar()
        {

            start = new aNode();
            goal = new aNode();

            openList = new List<aNode>();
            closedList = new List<aNode>();
            path = new List<aNode>();
        }

        public List<int[]> getPath()
        {
            List<int[]> solution = new List<int[]>();

            for(int i = 0; i < path.Count; i ++)
            {
                int[] temp = new int[2];
                temp[0] = path[i].x;
                temp[1] = path[i].y;
                solution.Add(temp);
            }

            return solution;
        }

        public void printPath()
        {
            if (path != null)

                Console.WriteLine("The solution path is:");

            for(int i = 0; i < path.Count; i++)
            {
                Console.WriteLine("[" + path[i].x + "," + path[i].y + "]");
            }
        }

        public void setInfluenceMap(int[,] im)
        {
            influenceMap = im;
        }

        public int[] returnFirst(int bx, int by, int[,] goals)
        {
            int[] solution = new int[2];
            int[] goal = new int[2];
            int solutionValue = 11111111;
            int currentValue = 0;

            // Determine the top value path and use that one.

            for (int n = 0; n < 1; n++)
            {
                if (goals[n, 0] == bx && goals[n, 1] == by) continue;
                calculatePath(bx, by, goals[n, 0], goals[n, 1]);
                Console.WriteLine("At:" + bx + "," + by + "  G: " + goals[n, 0] + "," + goals[n, 1]);
                for (int i = 0; i < path.Count; i++)
                    currentValue += influenceMap[path[i].x, path[i].y];

                if (currentValue < solutionValue)
                {
                    solutionValue = currentValue;

                    Console.WriteLine("Path[0].x = " + path[0].x);

                    solution[0] = path[0].x;
                    solution[1] = path[0].y;

                    goal[0] = goals[n, 0];
                    goal[1] = goals[n, 1];
                }

                currentValue = 0;
            }

            Console.WriteLine("At:" + bx + "," + by + "  First Step is: " + solution[0] + "," + solution[1] + "  G: " + goal[0] + "," + goal[1]);
            return solution;
        }

        public void calculatePath(int bx, int by, int ex, int ey)
        {

            start.x = bx;
            start.y = by;
            start.fromX = bx;
            start.fromY = by;
            start.fScore = 1;
            goal.x = ex;
            goal.y = ey;
            goal.fScore = 1;

            openList = new List<aNode>();
            closedList = new List<aNode>();

            openList.Add(start);

            while (openList.Count != 0)
            {
                aNode current = getBestNode();

                if (equals(current, goal))
                {
                   goalFound = true;
                   generatePath(current);
                   path.Reverse();
                    break;
                }
                else
                {

                    List<aNode> possibleNeighbors = getSuccessors(current);

                    for (int i = 0; i < possibleNeighbors.Count; i++)
                    {
                        aNode n = possibleNeighbors[i];

                        if (inClosed(n)) continue;

                        // Know where it came from so it can be traced back.
                        n.fromX = current.x;
                        n.fromY = current.y;

                        n.gScore = current.gScore + 1;
                        n.hScore = influenceMap[n.x, n.y];
                        n.fScore = n.gScore + n.hScore;
                        
                        if (!inOpen(n)) openList.Add(n);

                    }
                    closedList.Add(current);
                    removeFromOpen(current);

                }

               // printList(openList, "Open List");
               // printList(closedList, "Closed List");
            }
        }

        private void removeFromOpen(aNode node)
        {
            for(int i = 0; i < openList.Count; i++)
                if(equals(openList[i], node))
                    openList.RemoveAt(i);
        }

        private void printList(List<aNode> list, String title)
        {
            Console.WriteLine("-" + title + "-");
            for (int i = 0; i < list.Count; i++)
            {
                Console.WriteLine(printNode(list[i]) + " from " + list[i].fromX + " " + list[i].fromY);
            }
            Console.WriteLine("--");

  
            }

        private String printNode(aNode node)
        {
            return "[" + node.x + "," + node.y + "]";
        }


        private aNode generatePath(aNode currentNode)
        {
 
            if (isStart(currentNode))
                return currentNode;
            else
            {
               path.Add(currentNode);
               return generatePath(getPrevious(currentNode));
            }
        }

        private Boolean equals(aNode nodeA, aNode nodeB)
        {
            return (nodeA.x == nodeB.x && nodeA.y == nodeB.y);
        }

        private aNode getPrevious(aNode node)
        {

            for (int i = 0; i < closedList.Count; i++)
                if (node.fromX == closedList[i].x &&
                    node.fromY == closedList[i].y)
                {
                    return closedList[i];
                }

            for (int i = 0; i < openList.Count; i++)
                if (node.fromX == openList[i].x &&
                    node.fromY == openList[i].y)
                {
                    return openList[i];
                }

            //Should never get here;
            Console.WriteLine("Previous Not found");

            return new aNode();
        }

        private Boolean isStart(aNode node)
        {
            return (equals(node, start));
        }

        private Boolean inOpen(aNode n)
        {
            foreach (aNode temp in openList)
                if (equals(temp, n))
                    return true;

            return false;
        }

        private Boolean inClosed(aNode n)
        {
            for(int i = 0; i < closedList.Count; i ++)
                if (equals(closedList[i], n))
                    return true;

            return false;
        }

        private List<aNode> getSuccessors(aNode currentNode)
        {
            List<aNode> solution = new List<aNode>();

            aNode temp = new aNode();
            temp.x = currentNode.x + 1;
            temp.y = currentNode.y;
            if(isValidNode(temp.x, temp.y))  solution.Add(temp);

            temp.x = currentNode.x - 1;
            temp.y = currentNode.y;
            if (isValidNode(temp.x, temp.y)) solution.Add(temp);

            temp.x = currentNode.x;
            temp.y = currentNode.y + 1;
            if (isValidNode(temp.x, temp.y)) solution.Add(temp);

            temp.x = currentNode.x;
            temp.y = currentNode.y -1;
            if (isValidNode(temp.x, temp.y)) solution.Add(temp);

            return solution;

           /* for (int x = currentNode.x - 1; x <= currentNode.x + 1; x++)
                for (int y = currentNode.y - 1; y <= currentNode.y + 1; y++)
                    if (isValidNode(x, y))
                    {
                        aNode temp = new aNode();
                        temp.x = x;
                        temp.y = y;
                        solution.Add(temp);
                    }

            return solution;*/
        }

        private Boolean isValidNode(int x, int y)
        {
            if (x >= width || x < 0 || y >= height || y < 0) return false;
            if (influenceMap[x, y] == -1) return false;
            foreach (aNode n in closedList)
                if (n.x == x && n.y == y)
                    return false;

            foreach (aNode n in openList)
                if (n.x == x && n.y == y)
                    return false;

            return true;
        }

        private aNode getBestNode()
        {
            aNode solution = new aNode();
            int min = 30000; // a big number;

            foreach (aNode n in openList)
            {
                if (n.fScore < min)
                {
                    min = n.fScore;
                    solution.x = n.x;
                    solution.y = n.y;
                    solution.fromX = n.fromX;
                    solution.fromY = n.fromY;
                }
            }

            return solution;
        }

        struct aNode
        {
            public int x;
            public int y;
            public int fromX;
            public int fromY;
            public int gScore;
            public int hScore;
            public int fScore;
        }

    }
}
 